#define _READ_VIDEO_CPP_

#include <iostream>
//#include "SMATModel.hh"
//#include "display.hh"
//#include "utils.hh"
#include <string.h>


#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "read_video.h"
#include "options.h"
#include "frame_header.hh"
#include "other_functions.h"
#include <colores.h>

#define _FILE_OFFSET_BITS 64

//un buffer con 20 imágenes almacenadas (20 de cada cámara, en total 40)
#define BUFFER_IMAGES 10

typedef enum {
    DC1394_COLOR_FILTER_RGGB = 512,
    DC1394_COLOR_FILTER_GBRG,
    DC1394_COLOR_FILTER_GRBG,
    DC1394_COLOR_FILTER_BGGR
} dc1394color_filter_t;

void * CaptureVideo_thread (void * params);
bool dc1394_bayer_Simple(const char * bayer, char * rgb, int sx, int sy, 
			 int tile);


int get_read_images(stereo_IplImage_t **imgs, video_params_t * p, bool liberar_a_willy)
{
    if (liberar_a_willy)
	//si pide una nueva imagen, suponemos que ya ha terminado con la antigua
	free_read_images(&p->imgs_in_use, p);

    //comprueba que haya una imagen disponible
    //pthread_mutex_lock(&p->mutex_use);
    while (p->buffer_IplImage_use.empty())
    {
	//pthread_mutex_unlock(&p->mutex_use);

	//si no hay imágenes disponibles, lo primero es desbloquear el hilo de lectura de imágenes
	//pthread_mutex_unlock(&mutex_read);
	if (p->quit == 1)
	    return 0;
	//printf("waitting\n");
	//fflush(stdout);
	usleep(2000);
	//pthread_mutex_lock(&p->mutex_use);
    }
    
    //en cuanto la haya, la saca de la pila 
    pthread_mutex_lock(&p->mutex_use);
    p->imgs_in_use = p->buffer_IplImage_use.front();
    p->buffer_IplImage_use.pop();
    p->use--;
    pthread_mutex_unlock(&p->mutex_use);

    //fprintf(stderr, "%3d free / %3d used.\n", p->free, p->use);
    //fflush(stderr);

    *imgs = p->imgs_in_use;

    std::cout << CGREEN_B_L << " --------------- Frame: " << (*imgs)->hleft->nframe << 
	" ----------------"<< CNORMAL << std::endl;
    printf("Frame %d-> fps: %5.2f, shutter %d.\n",(*imgs)->hleft->nframe, 
	   1000.0/difftime_ms((*imgs)->hleft->filltime, p->oldfilltimeL), 
	   (*imgs)->hleft->shutter_usec);
    printf("Frame %d-> fps: %5.2f, shutter %d.\n",(*imgs)->hright->nframe, 
	   1000.0/difftime_ms((*imgs)->hright->filltime, p->oldfilltimeR), 
	   (*imgs)->hright->shutter_usec);
    printf("difftime left-right frame: %5.2f ms\n", difftime_ms((*imgs)->hleft->filltime, 
								(*imgs)->hright->filltime));
    p->oldfilltimeR = (*imgs)->hright->filltime;
    p->oldfilltimeL= (*imgs)->hleft->filltime;
    
    
    return 1;
}

int free_read_images(stereo_IplImage_t **imgs, video_params_t*p)
{
    //pone en buffer en la pila de los disponibles otra vez
    pthread_mutex_lock(&p->mutex_free);
    p->buffer_IplImage_free.push(*imgs);
    p->free++;
    pthread_mutex_unlock(&p->mutex_free);

    return 1;
}



/*
  Este hilo se encarga de leer las imágenes almacenadas en el video, e ir preparándolas en una cola de imágenes
*/
void * ReadVideo_thread (void * params)
{
    int i;
    stereo_IplImage_t buffer_imgs[BUFFER_IMAGES];
    stereo_IplImage_t *imgs;
    video_params_t * p = (video_params_t *)params;

    CvCapture *videoI = NULL;
    CvCapture *videoD = NULL;
    FILE *rawvideo = NULL;
    FILE *rawvideo2 = NULL;
    CvSize video_size;
    CvSize video_size2;
    CvSize video_size0;
    int video_channels;
    int difft;
    unsigned long readImageSize;

    p->free=0;
    p->use=0;
    
    pthread_mutex_unlock(&p->mutex_use);

    //abre los archivos con los videos y lee las dos primeras imágenes
    switch (p->video_format)
    {
    case ra2_video:
	//en caso de ser un fichero en formato ra2
	rawvideo = fopen(p->videoI_name, "r");
	fread(&video_size, 1, sizeof(CvSize), rawvideo);
	printf("width: %d, height: %d\n", video_size.width, video_size.height);
	fflush(stdout);
	video_channels = 1;
	
	//crea las dos primeras imágenes
	buffer_imgs[0].left = cvCreateImage(video_size, IPL_DEPTH_8U, video_channels );
	buffer_imgs[0].right = cvCreateImage(video_size, IPL_DEPTH_8U, video_channels );

	//se coloca en el primer frame a leer
	fseeko(rawvideo, buffer_imgs[0].left->imageSize*(unsigned long)p->iniframe*2 + sizeof(CvSize), SEEK_SET);

	//lee las dos primeras imágenes
	fread((void*) buffer_imgs[0].left->imageData, buffer_imgs[0].left->imageSize, 1, rawvideo);
	fread((void*) buffer_imgs[0].right->imageData, buffer_imgs[0].right->imageSize, 1, rawvideo);
	break;
    case raw_video:
	//en caso de ser dos ficheros en formato raw
	frame_header head, head2;

	rawvideo = fopen(p->videoI_name, "r");
	rawvideo2 = fopen(p->videoD_name, "r");
	
	fread(&head, sizeof(frame_header), 1, rawvideo);
	video_size.width = head.width;
	video_size.height = head.height;

	fread(&head2, sizeof(frame_header), 1, rawvideo2);
	video_size2.width = head2.width;
	video_size2.height = head2.height;
	
	video_size0.width = video_size.width;
	video_size0.height = 700;

	printf("Video left:  width: %d, height: %d\n", video_size.width, video_size.height);
	printf("Video right: width: %d, height: %d\n", video_size2.width, video_size2.height);
	fflush(stdout);
	video_channels = 1;
	
	//crea las dos primeras imágenes
	buffer_imgs[0].left = cvCreateImage(video_size0, IPL_DEPTH_8U, video_channels );
	buffer_imgs[0].right = cvCreateImage(video_size0, IPL_DEPTH_8U, video_channels );
	
	readImageSize = video_size.width * video_size.height;

	//elimina las imágenes que no valgan al principio del video
	if (p->iniframe == 0)
	{
	    while (head.nframe > 1)
	    {	
		fseeko(rawvideo, readImageSize- sizeof(head), SEEK_CUR);
		fread(&head, sizeof(frame_header), 1, rawvideo);
		printf("Video 1, Skipping frame %d\n", head2.nframe);
	    }   
	    while (head2.nframe > 1)
	    {
		fseeko(rawvideo2, readImageSize- sizeof(head2), SEEK_CUR);
		fread(&head2, sizeof(frame_header), 1, rawvideo2);
		printf("Video 2, Skipping frame %d\n", head2.nframe);
	    }   
	    //se coloca en el primer frame a leer
	    fseeko(rawvideo, - sizeof(head), SEEK_CUR);
	    fseeko(rawvideo2, - sizeof(head2), SEEK_CUR);
	}
	else
	{
	    //se coloca en el primer frame a leer
	    fseeko(rawvideo, readImageSize*p->iniframe, SEEK_SET);
	    fseeko(rawvideo2, readImageSize*p->iniframe, SEEK_SET);
	}

	//lee las dos primeras imágenes
	//buffer_imgs[0].hleft = read_image(buffer_imgs[0].left, rawvideo);
	//buffer_imgs[0].hright = read_image(buffer_imgs[0].right, rawvideo2);
	break;
    case avi_video:
	//abre los videos en formato avi, derecha e izquierda en videos distintos
	videoI = cvCreateFileCapture(p->videoI_name);
	videoD = cvCreateFileCapture(p->videoD_name);

	if(!videoD || !videoI)
	{
	    fprintf(stderr, "No video file. Exiting\n"); fflush(stderr);
	    return (void*)-1;
	}

	//lee las dos primeras imágenes
	buffer_imgs[0].left = cvCloneImage(cvQueryFrame(videoI));
	buffer_imgs[0].right = cvCloneImage(cvQueryFrame(videoD));
	video_size = cvGetSize(buffer_imgs[0].left);
	video_channels = buffer_imgs[0].left->nChannels;
	break;
    case no_format:
	printf("Error de formato del video\n");
	break;
    }

    //mete en el buffer el pimer set de imágenes, y desbloquea el semáforo
    p->buffer_IplImage_free.push(&buffer_imgs[0]);
    p->use++;

    //espera un poco para la inicialización del resto de cosas
    usleep(2000);

    //genera el buffer de imágenes donde leer del archivo
    for (i = 1; i<BUFFER_IMAGES; i++)
    {
	buffer_imgs[i].left = cvCreateImage(video_size0, IPL_DEPTH_8U, video_channels );
	buffer_imgs[i].right = cvCreateImage(video_size0, IPL_DEPTH_8U, video_channels );
	p->buffer_IplImage_free.push(&buffer_imgs[i]);
	p->free++;
    }
    

    // bucle que va leyendo las imágenes y dejándolas en el buffer
    while (!p->quit)
    {
	//espera a que puede ejecutar la tarea
	//pthread_mutex_lock(&mutex_read);

	//si no hay buffer disponible, no lee, y espera a que haya
	while (p->buffer_IplImage_free.empty())
	{
	    //pthread_mutex_unlock(&mutex_read);
	    usleep(100000);
	    //pthread_mutex_lock(&mutex_read);
	}

	
	//ya hay buffer libre. Coge el siguiente disponible
	pthread_mutex_lock(&p->mutex_free);
	imgs = p->buffer_IplImage_free.front();
	p->buffer_IplImage_free.pop();
	p->free--;
	pthread_mutex_unlock(&p->mutex_free);

	//lee las imágenes del fichero
	switch (p->video_format)
	{
	case avi_video:
	    IplImage *img1, *img2;
	    img1 = cvQueryFrame(videoI);
	    img2 = cvQueryFrame(videoD);

	    if(!CV_IS_IMAGE(img1))
	    {
		printf("End of video\n");
		fflush(stdout);
		break;
	    }
	    cvCopy(img1, imgs->left);
	    cvCopy(img2, imgs->right);
	    break;
	case raw_video:
	    imgs->hleft = read_image(imgs->left, rawvideo, video_size);
	    imgs->hright = read_image(imgs->right, rawvideo2, video_size);
	    difft = (int)difftime_ms(imgs->hleft->filltime, imgs->hright->filltime);
	    while (abs(difft) > 15)
	    {
		if (difft > 0)
		    imgs->hright = read_image(imgs->right, rawvideo2, video_size);
		else 
		    imgs->hleft = read_image(imgs->left, rawvideo, video_size);

		difft = (int)difftime_ms(imgs->hleft->filltime, imgs->hright->filltime);

		if (feof(rawvideo) || feof(rawvideo2))
		{
		    printf("End of video\n");
		    fflush(stdout);
		    p->quit = 1;
		    break;
		}
	    }
	    
	    break;
	case ra2_video:
	    fread((void*) imgs->left->imageData, imgs->left->imageSize, 1, rawvideo);
	    fread((void*) imgs->right->imageData, imgs->right->imageSize, 1, rawvideo);
	    
	    if(!CV_IS_IMAGE(imgs->left))
	    {
		printf("End of viedo\n");
		fflush(stdout);
		break;
	    }
	    break;
	case no_format:
	    printf("Error de formato del video\n");
	    break;
	}

	if (p->quit)
	    break;
	//mete en buffer en la pila para que sea usado por el hilo consumidor
	pthread_mutex_lock(&p->mutex_use);
	p->buffer_IplImage_use.push(imgs);
	p->use++;
	pthread_mutex_unlock(&p->mutex_use);

	//introduce un pequeño retardo en la lectura de imágnes
	//pthread_mutex_unlock(&mutex_read);
	usleep(500);
    }

    
    while (!p->buffer_IplImage_use.empty())
    {
	usleep(10000);
    }

    for (i = 0; i<BUFFER_IMAGES; i++)
    {
	cvReleaseImage(&buffer_imgs[i].left);
	cvReleaseImage(&buffer_imgs[i].right);
    }

    if (videoI)
	cvReleaseCapture(&videoI);
    if (videoD)
	cvReleaseCapture(&videoD);
    if (rawvideo)
	fclose(rawvideo);

    if (p->videoI_name)
	free(p->videoI_name);
    if (p->videoD_name)
	free(p->videoD_name);

    printf("fin de la captura de video\n");
    fflush(stdout);
    return NULL;
}


/*
  lanza el hilo de lectura de las imágenes
*/

int start_ReadVideo_thread(video_params_t * p, bool use_camera, 
			   const char * video_name, const char *format, int iniframe)
{
    pthread_t th;

    if(strlen(video_name) == 0)
    {
	printf("Not enough videos.\n");
	exit(-1);
    }
    p->videoI_name = (char *)malloc(strlen(video_name)+20);
    strcpy(p->videoI_name, video_name);
    p->videoD_name = 0;

    char *extension = rindex(p->videoI_name, '.');

    //busca el formato del archivo de video
    p->video_format = no_format;
    if (strcasecmp(format, "raw") == 0)
    {
	p->video_format = raw_video;
    }
    else if (strcasecmp(format, "ra2") == 0)
    {
	p->video_format = ra2_video;
    }
    else if (strcasecmp(format, "avi") == 0)
    {
	p->video_format = avi_video;
    }
    
    if (p->video_format == no_format)
    {
	if (extension == NULL)
	    p->video_format = raw_video;
	else if (strcasecmp(extension, ".cal") == 0)
	    p->video_format = raw_video;
	else if (strcasecmp(extension, ".avi") == 0)
	    p->video_format = avi_video;
	else if (strcasecmp(extension, ".raw") == 0)
	    p->video_format = raw_video;
	else if (strcasecmp(extension, ".ra2") == 0)
	    p->video_format = ra2_video;
	else
	    p->video_format = no_format;
    }



    printf("video format: %d\n", p->video_format);
    //extrae los nombres de los videos, según el formato indicado

    switch (p->video_format)
    {
    case raw_video:
	if (extension == NULL)
	    strcat(p->videoI_name, "1.raw");
	else if (strcasecmp(extension, ".cal") == 0)
	    strcpy(extension, ".raw");

	p->videoD_name = (char *)malloc(strlen(p->videoI_name)+20);
	strcpy(p->videoD_name, p->videoI_name);
	extension = rindex(p->videoD_name, '.');

	if (extension[-1] == '2')
	{
	    extension = rindex(p->videoI_name, '.');
	    extension[-1] = '1';
	}
	else
	    extension[-1] = '2';
	break;
    case ra2_video:
	if (extension == NULL)
	    strcat(p->videoI_name, ".ra2");
	else if (strcasecmp(extension, ".cal") == 0)
	    strcpy(extension, ".ra2");
	break;

    case avi_video:
	if (extension == NULL)
	    strcat(p->videoI_name, "1.avi");
	else if (strcasecmp(extension, ".cal") == 0)
	    strcpy(extension, ".avi");
	break;

    case no_format:
	printf("Error de formato del video.\n");
	free(p->videoI_name);
	return 0;
	break;
    }
    
    printf("Video 1:     %s\n", p->videoI_name);
    if (p->videoD_name)
	printf("Video 2:     %s\n", p->videoD_name);
	

    pthread_mutex_init(&p->mutex_use, NULL);
    pthread_mutex_init(&p->mutex_free, NULL);
    pthread_mutex_init(&mutex_read, NULL);
    p->quit = false;
    if (iniframe < 0)
    {
	char buffer[400];
	strcpy(buffer, p->videoI_name);
	extension = rindex(buffer, '.');
	strcpy(extension, ".ini");
	FILE *fini = fopen(buffer, "r");
	if (!fini)
	    iniframe = 0;
	else
	{
	    fscanf(fini, "%d", &iniframe);
	    if (iniframe < 0) 
		iniframe = 0;
	}
    }
    p->iniframe = iniframe;
    
    //empieza con el mutex bloqueado, hasta que carge las primeras imágenes
    pthread_mutex_lock(&p->mutex_use);

    //lanza los hilos que hacen el SMAT y el RANSAC
    if (!use_camera)
    {
 	if (pthread_create(&th, NULL, ReadVideo_thread, (void*)p))
	{
	    if (p->videoI_name)
		free(p->videoI_name);
	    if (p->videoD_name)
		free(p->videoD_name);
	    return -1;
	}
    }
    else
    {
	printf("Video grabbing from camera not supported\n");
	{
	    if (p->videoI_name)
		free(p->videoI_name);
	    if (p->videoD_name)
		free(p->videoD_name);
	    return -1;
	}
    }
    p->pthread = th;

    return iniframe;
}




 
